/* UTF8 Encoding */
/*-----------------------------------------------------------------------
|                            FILE DESCRIPTION                           |
-----------------------------------------------------------------------*/
/*----------------------------------------------------------------------
  - File name     : track_config.hpp
  - Author        : liuzhihua (liuzhihuawy@163.com)
  - Update date   : 2023.08.01
  -	Brief         : image track config
  - Version       : v0.1
-----------------------------------------------------------------------*/
#ifndef TRACK_CONFIG_HPP
#define TRACK_CONFIG_HPP

/*-----------------------------------------------------------------------
|                               INCLUDES                                |
-----------------------------------------------------------------------*/
#include <stdint.h>
#include "elist.h"
#include "bsp_uart.h"
#include <math.h>
/*-----------------------------------------------------------------------
|                                DEFINES                                |
-----------------------------------------------------------------------*/

typedef enum trackObjColorID
{
    TRACK_COLOR_RED = 0,
    TRACK_COLOR_GREEN = 1,
    TRACK_COLOR_BLUE = 2,
    TRACK_COLOR_YELLOW = 3,
    TRACK_COLOR_NONE = 4,
}trackObjColorID;
typedef enum trackObjShapeID
{
    TRACK_SHAPE_RECTANGLE = 0,
    TRACK_SHAPE_TRIANGLE = 1,
    TRACK_SHAPE_CIRCLE = 2,
    TRACK_SHAPE_NONE = 3,
}trackObjShapeID;
extern const char trackObjColor[TRACK_COLOR_NONE+1][10];
extern const char trackObjShape[TRACK_SHAPE_NONE+1][10];

typedef struct trackResult_t
{
    uint8_t color;
    uint16_t sx; //start x position
    uint16_t sy; //start y position
    uint8_t w;
    uint8_t h;
    uint8_t shape;
    float solidity;
}trackResult_t;

typedef struct trackConfig_t
{
    uint16_t color;
    uint16_t shape;
    uint16_t xSizeMin, xSizeMax;
    uint16_t ySizeMin, ySizeMax;
    uint16_t cxStart, cxEnd; // center x range
    uint16_t cyStart, cyEnd; // center y range
}trackConfig_t;

class trackResultCollecter
{
    private:

    public:
        static const int resultMax = 12;
        trackConfig_t *trackConfig;
        uint8_t trackConfigSize; // TODO:Multi Concfig
        trackResult_t results[resultMax]; //避免使用malloc
        uint8_t objCounter[resultMax];
        short cnt;

        trackResultCollecter(void)
        {
            trackConfig = NULL;
            memset(results, 0, sizeof(results));
            memset(objCounter, 0, sizeof(objCounter));
        }
        bool isSameResult(const trackResult_t *src1, const trackResult_t *src2) const
        {
            if (src1->color != src2->color) return false;
            if (fabs(src1->solidity - src2->solidity) > 0.15f) return false;
            int cx1 = src1->sx + (src1->w>>1);
            int cy1 = src1->sy + (src1->h>>1);
            int cx2 = src2->sx + (src2->w>>1);
            int cy2 = src2->sy + (src2->h>>1);
            if (2*abs(cx1 - cx2) > src1->w || 2*abs(cy1 - cy2) > src1->h) return false;
            return true;

        }
        int findSameResult(const trackResult_t *src) const
        {
            if (src == NULL) return -1;
            for (size_t i = 0; i < cnt; i++)
            {
                if (isSameResult(&results[i], src) == false) continue;
                return i;
            }
            return -1;
        }
        int findMaxCountObj(void) const
        {
            if (cnt == 0) return -1;
            int maxIndex = 0;
            for (size_t i = 0; i < cnt; i++)
            {
                if (objCounter[maxIndex] < objCounter[i])
                    maxIndex = i;
            }
            return maxIndex;
        }
        trackObjShapeID shapeCondition(float solidity)
        {
            if (solidity < 0.8) return TRACK_SHAPE_TRIANGLE;
            else if (solidity < 0.9) return TRACK_SHAPE_CIRCLE;
            else if (solidity <= 1) return TRACK_SHAPE_RECTANGLE;
			return TRACK_SHAPE_NONE;
        }
        bool addResult(const trackResult_t *src)
        {
            int sameIndex;
            if (src == NULL) return false;
            sameIndex = findSameResult(src);
            if (sameIndex >= 0)
            {
                if (objCounter[sameIndex] < 255)
                    objCounter[sameIndex]++;
                return false;
            }
            if (cnt >= resultMax)
            {
				return false;
            }
            //shape condition
            memcpy(&results[cnt], src, sizeof(results[0]));
            results[cnt].shape = (uint8_t)shapeCondition(src->solidity);
            cnt++;
			return true;
        }

        trackResult_t *readResult(uint16_t index)
        {
            if (cnt <= index) return NULL;
            return &results[index];
        }


        bool matchResult(const trackConfig_t *config, const trackResult_t *src) const
        {
            if (config == NULL) return false;
            if (config->color != src->color || config->shape != src->shape) return false;
            if (config->xSizeMax < src->w || config->xSizeMin > src->w) return false;
            if (config->ySizeMax < src->h || config->ySizeMin > src->h) return false;
            int cx = src->sx + (src->w>>1);
            int cy = src->sy + (src->h>>1);
            if (config->cxEnd < cx || config->cxStart > cx) return false;
            if (config->cyEnd < cy || config->cyStart > cy) return false;
            return true;
        }

        int findResult(const trackConfig_t *config) const
        {
            for (size_t i = 0; i < cnt; i++)
            {
                if (matchResult(config, &results[i]))
                    return i;
            }
            return -1;
        }

        void deleteResult(trackConfig_t *config)
        {
            if (config == NULL) return;
            for (size_t i = 0; i < cnt;)
            {
                if (matchResult(config, &results[i]) == false)
                {
                    i++;
                    continue;
                }
                if (cnt - i - 1 > 0)
                    memmove(&results[i], &results[i+1], cnt - i - 1);
                cnt--;
            }
        }

        void keepResult(trackConfig_t *config)
        {
            if (config == NULL)
            {
                cnt = 0;
                return;
            }
            for (size_t i = 0; i < cnt;)
            {
                if (matchResult(config, &results[i]) == true)
                {
                    i++;
                    continue;
                }
                if (cnt - i - 1 > 0)
                    memmove(&results[i], &results[i+1], cnt - i - 1);
                cnt--;
            }
        }

        void clearResult(void)
        {
            // memset(results, 0, sizeof(results));
            cnt = 0;
            memset(objCounter, 0, sizeof(objCounter));
        }

        void print(const trackResult_t *result) const
        {
            Queue_Printf(&htx1, "TrackObj:%s,%d,%d,%d,%d,%s,%.2f;\n\r",
                        trackObjColor[result->color],
                        result->sx, result->sy,
                        result->w, result->h,
                        trackObjShape[result->shape],
                        result->solidity);
            
        }

        void printResults(void) const
        {
            Queue_Printf(&htx1, "\n\r----------------------TrackResults----------------------\n\r");
            Queue_Print(&htx1, "[No]: x , y ,w ,h ,soli,cnt, color,  shape\n\r");
            for (size_t i = 0; i < cnt; i++)
            {
                Queue_Printf(&htx1, "[%02d]:%3d,%3d,%2d,%2d,%.2f,%3d,%6s,%9s;\n\r",
                            i,
                            results[i].sx, results[i].sy,
                            results[i].w, results[i].h,
                            results[i].solidity,
                            objCounter[i],
                            trackObjColor[results[i].color],
                            trackObjShape[results[i].shape]
                            );
            }
            Queue_Printf(&htx1, "Total:%d\n\r", cnt);
        }
};
/*-----------------------------------------------------------------------
|                                extern                                 |
-----------------------------------------------------------------------*/

extern trackResult_t trackResult;
extern trackResultCollecter trackReceive;
extern trackConfig_t trackconfig;
void colorTrackInit(void);
void startColorTrack(int video_id, trackConfig_t *config);
#endif
